// KrautWiimote.cpp : Defines the entry point for the console application.
//

#include "WiiYourself/wiimote.h"
#include "bluetooth.h"
#include "KrautMobil.h"
#include "error.h"
#include <time.h>
#include <cstdlib> 

static Bluetooth nxt;
signed char currentSpeed = 0;
signed char currentAngle = 0;
signed char steeringThreshold = 5;


// ------------------------------------------------------------------------------------
//  state-change callback example (we use polling for everything else):
// ------------------------------------------------------------------------------------
void on_state_change (wiimote			      &remote,
                      state_change_flags  changed,
                      const wiimote_state &new_state)
{
   static time_t lastSteeringRequestTime = 0;


   if ( (changed & ORIENTATION_CHANGED) == ORIENTATION_CHANGED || 
        (changed & ACCEL_CHANGED) == ACCEL_CHANGED )
   {
      //if ( time(0) - lastSteeringRequestTime > 0) // only sends 1 request per second
      {
         //lastSteeringRequestTime = time(0);
         signed char wiimoteAngle = remote.Acceleration.Orientation.Pitch;
         if ( wiimoteAngle > 100 )  wiimoteAngle = 100;
         if ( wiimoteAngle < -100 ) wiimoteAngle = -100;

         if ( abs(wiimoteAngle - currentAngle ) >= steeringThreshold )
         {
            currentAngle = wiimoteAngle;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_STEER,
               currentAngle,
               KRAUT_MSG_END_BYTE
            };
            nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
         }
      }
   }

   if ( changed & CONNECTED )
   {
		if(new_state.bExtension)
			remote.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR_EXT); // no IR dots
		else
			remote.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR);		//    IR dots
      remote.SetLEDs(0x0F);
   }

   if ( (changed & BUTTONS_CHANGED) == BUTTONS_CHANGED )
   {
      //if ( remote.Button.B() )
      {
         if ( remote.Button.Two() )
         {
            // accelerate
            currentSpeed += 10;
            if ( currentSpeed > 100 ) currentSpeed = 100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_TRQ,
               currentSpeed,
               KRAUT_MSG_END_BYTE
            };
            nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
         }
         else if ( remote.Button.One())
         {
            // brake
            currentSpeed -= 10;
            if ( currentSpeed < -100 ) currentSpeed = -100;
            char msg[] = {
               KRAUT_MSG_START_BYTE,
               KRAUT_CMD_REQ_TRQ,
               currentSpeed,
               KRAUT_MSG_END_BYTE
            };
            nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
         }
      }
      if (remote.Button.Home())
      {
         // Full stop
         currentSpeed = 0;
         char msg[] = {
            KRAUT_MSG_START_BYTE,
            KRAUT_CMD_REQ_TRQ,
            currentSpeed,
            KRAUT_MSG_END_BYTE
         };
         nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);

         // Now calibrate steering
         char msg2[] = {
            KRAUT_MSG_START_BYTE,
            KRAUT_CMD_REQ_STEER_CALIBRATE,
            0,
            KRAUT_MSG_END_BYTE
         };
         nxt.send((unsigned char *)msg2, KRAUT_MSG_TOTAL_LEN);
      }
   }
      //if ( remote.Button.B() )
      //{
      //   if (currentSpeed != 0)
      //   {
      //      // Full stop
      //      currentSpeed = 0;
      //      char msg[] = {
      //         KRAUT_MSG_START_BYTE,
      //         KRAUT_CMD_REQ_TRQ,
      //         currentSpeed,
      //         KRAUT_MSG_END_BYTE
      //      };
      //      nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
      //   }
      //}

}

//-----------------------------------------------------------------------------
void SendSteeringCmd(wiimote & wii)
//-----------------------------------------------------------------------------
{
   // Target can only relative...
   if ( wii.Acceleration.Orientation.Pitch > 0 )
   {
      currentAngle = 10;
   }
   else
   {
      currentAngle = -10;
   }
   if ( currentAngle > 100 )  currentAngle = 100;
   if ( currentAngle < -100 ) currentAngle = -100;
   char msg[] = {
      KRAUT_MSG_START_BYTE,
      KRAUT_CMD_REQ_STEER,
      currentAngle,
      KRAUT_MSG_END_BYTE
   };
   nxt.send((unsigned char *)msg, KRAUT_MSG_TOTAL_LEN);
}

int main(int argc, char* argv[])
{
   bool nxtConnected = false;

   if ( 2 != argc )
   {
	   std::cerr << "Usage: " << argv[0] << " <port>" << std::endl;
	   std::cerr << "  <port> COM port number to use to connect to NXT" << std::endl;
	   return -1;
   }

   int nxtPort = atoi(argv[1]);

   std::cout << "Waiting for NXT (using port #" << nxtPort << ")" << std::endl;
   while (!nxtConnected)
   {
      try 
      {
         nxt.connect(nxtPort);
         nxtConnected = true;
      }
      catch(Nxt_exception e)
      {
         nxtConnected = false;
      }
   }
   std::cout << "NXT connected: " << nxtConnected << std::endl;

   wiimote wii;

   wii.CallbackTriggerFlags = (state_change_flags)(CONNECTED | ORIENTATION_CHANGED | ACCEL_CHANGED | BUTTONS_CHANGED | CONNECTION_LOST);
   wii.ChangedCallback = on_state_change;
   
   try 
   {
	   if (!wii.Connect())
	   {
        std::cerr << "Connection to wiimote failed" << std::endl;
		  return -1;
	   }
   }
   catch(exception e)
   {
	   if (!wii.IsConnected())
	   {
		   std::cerr << "Connection to wiimote failed: " << e.what() << std::endl;
		   return -1;
	   }
   }

   std::cout << "Wiimote connected" << std::endl;

   while(!wii.ConnectionLost())
   {
		// IMPORTANT: the wiimote state needs to be refreshed each pass
		while(wii.RefreshState() == NO_CHANGE)
      {
			Sleep(100); // don't hog the CPU if nothing changed
      }
   }

	return 0;
}

